use std::ops::DerefMut;
use tokio::sync::Mutex;
use crate::{error::AppErr, global::APP, store::track::{self, TrackCmd}};
use super::cmd::{move_gripper::MoveGripper, Cmd};



pub struct Adjust {
    name: String,
    tracks: Vec<TrackCmd>,
}

static ADJUST: Mutex<Option<Adjust>> = Mutex::const_new(None);

pub async fn adj_start(name: String) -> Result<(), AppErr> {
    if track::is_exists(&name).await? {
        return Err(AppErr::Static("轨迹名称已经存在"));
    }
    *ADJUST.lock().await = Some(Adjust { name, tracks: Vec::new() });
    Ok(())
}


pub async fn adj_save() -> Result<(), AppErr> {
    let mut adjust = ADJUST.lock().await;
    match adjust.deref_mut() {
        Some(adjust) => {
            track::save(&adjust.name, &adjust.tracks).await?;
        },
        None => return Err(AppErr::Static("没有轨迹数据")),
    };
    adjust.take();
    Ok(())
}


pub async fn adj_movej_cur(speed: f32, acc: f32, bland_t: f32) -> Result<(), AppErr> {
    let mut adjust = ADJUST.lock().await;
    match adjust.deref_mut() {

        None => return Err(AppErr::Static("请先开始示教")),

        Some(adjust) => {
            let status = APP.robot_status().await.ok_or(AppErr::Static("没用收到状态"))?;
            let movej = status.movej(speed, acc, bland_t);
            let track_cmd = TrackCmd {
                cmd_id: movej.cmd_id(),
                data: movej.to_string()
            };
            adjust.tracks.push(track_cmd);
        }
    }

    Ok(())
}

pub async fn adj_movel_cur(speed: f32, acc: f32, bland_r: f32) -> Result<(), AppErr> {
    let mut adjust = ADJUST.lock().await;
    match adjust.deref_mut() {

        None => return Err(AppErr::Static("请先开始示教")),

        Some(adjust) => {
            let status = APP.robot_status().await.ok_or(AppErr::Static("没用收到状态"))?;
            let movej = status.movel(speed, acc, bland_r);
            let track_cmd = TrackCmd {
                cmd_id: movej.cmd_id(),
                data: movej.to_string()
            };
            adjust.tracks.push(track_cmd);
        }
    }

    Ok(())
}

pub async fn adj_jaw(pos: i32, tor: i32) -> Result<(), AppErr> {
    let mut adjust = ADJUST.lock().await;
    match adjust.deref_mut() {

        None => return Err(AppErr::Static("请先开始示教")),

        Some(adjust) => {
            let movej = MoveGripper::new(pos, tor);
            let track_cmd = TrackCmd {
                cmd_id: movej.cmd_id(),
                data: movej.to_string()
            };
            adjust.tracks.push(track_cmd);
        }
    }

    Ok(())
}



